#ifndef SPLINE_HPP
#define SPLINE_HPP

#include <vector>
#include <stdexcept>
#include <iostream>
#include <iomanip>
#include "Function.hpp"
#include "lapacke.h"

const double eps = 1e-4; // Threshold for determining if certain floating-point numbers are equal

/**
 * @brief Check if a vector is strictly increasing
 * @param vec Input vector
 * @return Whether the vector is strictly increasing
 */
bool isIncreasing(const std::vector<double>& vec) {
    for (size_t i = 1; i < vec.size(); ++i) {
        if (vec[i] <= vec[i - 1]) {
            return false;
        }
    }
    return true;
}

/**
 * @brief Solve the linear system Ax = b and return the solution vector x
 * @param A Coefficient matrix
 * @param b Constant vector
 * @return Solution vector x
 */
std::vector<double> LinearSolve(const std::vector<double>& A, const std::vector<double>& b) {
    int N = b.size(); // Dimension of the system
    if (A.size() != N * N) {
        throw std::invalid_argument("Matrix A must be a square matrix with the same dimension as vector b.");
    }

    std::vector<double> x(b); // Initialize solution vector x as a copy of vector b
    std::vector<int> ipiv(N); // Store pivot indices
    int info;

    // Call LAPACK function to solve the linear system
    info = LAPACKE_dgesv(LAPACK_ROW_MAJOR, N, 1, const_cast<double*>(A.data()), N, ipiv.data(), x.data(), 1);

    // Check the return value of the LAPACK function
    if (info != 0) {
        throw std::runtime_error("Failed to solve linear system");
    }

    return x; // Return the solution vector x
}

/**
 * @brief Base class for splines
 */
class Spline {
protected:
    std::vector<double> knots_; ///< Knot vector
    int n_; ///< Degree of the spline
public:
    /**
     * @brief Constructor
     * @param knots Knot vector
     * @param n Degree
     */
    Spline(const std::vector<double>& knots, int n) : knots_(knots), n_(n) {
        if (!isIncreasing(knots)) {
            for (double i : knots) {
                std::cout << i << " ";
            }
            throw std::invalid_argument("Knots vector must be strictly increasing.");
        }
    }

    virtual ~Spline() = default;

    /**
     * @brief Pure virtual function to evaluate the spline at a given point x
     * @param x Input point
     * @return Value of the spline at x
     */
    virtual double evaluate(double x) const = 0;

    /**
     * @brief Get the knot vector
     * @return Knot vector
     */
    std::vector<double> getKnots() const {
        return knots_;
    }

    /**
     * @brief Get the degree of the spline
     * @return Degree
     */
    int getDegree() const {
        return n_;
    }
};

/**
 * @brief BSpline class, generated using coefficients. Subclasses generate coefficients using functions or function values.
 */
class BSpline : public Spline {
protected:
    std::vector<double> coefficients_; ///< Coefficient vector
public:
    /**
     * @brief Constructor, accepts n + N + 1 knots and N coefficients
     * @param knots Knot vector
     * @param n Degree
     * @param coefficients Coefficient vector
     */
    BSpline(const std::vector<double>& knots, int n, const std::vector<double>& coefficients)
        : Spline(knots, n) {
        if (knots.size() != coefficients.size() + n + 1) {
            for (double i : knots) {
                std::cout << i << " ";
            }
            std::cout << std::endl;
            for (double i : coefficients) {
                std::cout << i << " ";
            }
            std::cout << std::endl;
            throw std::invalid_argument("Invalid vector size!");
        }
        coefficients_.push_back(0.0); // Add a 0 at the beginning of coefficients
        coefficients_.insert(coefficients_.end(), coefficients.begin(), coefficients.end());
        coefficients_.insert(coefficients_.end(), n, 0.0); // Add n zeros at the end of coefficients
    }

    /**
     * @brief Recursive definition of the B base function
     * @param i Knot index
     * @param n Degree
     * @param x Input point
     * @param t Knot vector
     * @return B-spline value
     */
    static double B(int i, int n, double x, const std::vector<double>& t) {
        if (n == 0) {
            return (x > t[i - 1] && x <= t[i]) ? 1.0 : 0.0;
        } else {
            double denom1 = t[i + n - 1] - t[i - 1];
            double denom2 = t[i + n] - t[i];
            double term1 = 0.0;
            double term2 = 0.0;

            if (denom1 != 0.0)
                term1 = ((x - t[i - 1]) / denom1) * B(i, n - 1, x, t);
            if (denom2 != 0.0)
                term2 = ((t[i + n] - x) / denom2) * B(i + 1, n - 1, x, t);

            return term1 + term2;
        }
    }

    /**
     * @brief Implement the evaluate function to compute the B-spline value at x
     * @param x Input point
     * @return B-spline value
     */
    double evaluate(double x) const override {
        double result = 0.0;
        for (int i = 1; i < knots_.size() - n_; ++i) {
            result += coefficients_[i] * B(i, n_, x, knots_);
        }
        return result;
    }
};

/**
 * @brief LinearPPForm class
 */
class LinearPPForm : public Spline {
protected:
    std::vector<double> values_; ///< Function values
public:
    /**
     * @brief Constructor, accepts knot vector and function
     * @param knots Knot vector
     * @param func Function
     */
    LinearPPForm(const std::vector<double>& knots, const Function& func)
        : Spline(knots, 1), values_(knots.size()) {
        for (size_t i = 0; i < knots.size(); ++i) {
            values_[i] = func(knots[i]);
        }
    }

    /**
     * @brief Constructor, accepts knot vector and function values
     * @param knots Knot vector
     * @param values Function values
     */
    LinearPPForm(const std::vector<double>& knots, const std::vector<double>& values)
        : Spline(knots, 1), values_(values) {
        if (knots.size() != values.size()) {
            throw std::invalid_argument("Knots and values vectors must have the same size.");
        }
    }

    /**
     * @brief Implement the evaluate function to compute the piecewise polynomial value at x
     * @param x Input point
     * @return Piecewise polynomial value
     */
    double evaluate(double x) const override {
        
        int segment = -1;
        for (size_t i = 0; i < knots_.size() - 1; ++i) {
            if (x >= knots_[i] && x < knots_[i + 1]) {
                segment = i;
                break;
            }
        }
        if (segment == -1) {
            if (x < knots_.front() - eps || x > knots_.back() + eps) {
                throw std::out_of_range("x is out of the knots range.");
            } else if (x < knots_.front()) {
                segment = 0;
            } else {
                segment = knots_.size() - 2;
            }
        }

        double x0 = knots_[segment];
        double x1 = knots_[segment + 1];
        double y0 = values_[segment];
        double y1 = values_[segment + 1];
        double t = (x - x0) / (x1 - x0);
        return (1 - t) * y0 + t * y1;
    }
};

/**
 * @brief LinearBSpline class
 */
class LinearBSpline : public BSpline {
public:
    /**
     * @brief Constructor, accepts knot vector and function
     * @param knots Knot vector
     * @param func Function
     */
    LinearBSpline(const std::vector<double>& knots, const Function& func)
        : BSpline(ExtendKnots(knots), 1, std::vector<double>(knots.size())) {
        for (size_t i = 0; i < knots.size(); ++i) {
            coefficients_[i + 1] = func(knots[i]);
        }
    }

    /**
     * @brief Constructor, accepts knot vector and function values
     * @param knots Knot vector
     * @param values Function values
     */
    LinearBSpline(const std::vector<double>& knots, const std::vector<double>& values)
        : BSpline(ExtendKnots(knots), 1, values) {
        if (knots.size() != values.size()) {
            throw std::invalid_argument("Knots and values vectors must have the same size.");
        }
    }

private:
    /**
     * @brief Static method to extend the knot vector
     * @param knots Knot vector
     * @return Extended knot vector
     */
    static std::vector<double> ExtendKnots(const std::vector<double>& knots) {
        std::vector<double> extendedKnots(knots.size() + 2);
        double interval = knots[1] - knots[0];
        extendedKnots[0] = knots[0] - interval;
        std::copy(knots.begin(), knots.end(), extendedKnots.begin() + 1);
        extendedKnots.back() = knots.back() + interval;
        return extendedKnots;
    }
};

/**
 * @brief CompleteCubicPPForm class
 */
class CompleteCubicPPForm : public Spline {
protected:
    std::vector<double> values_; ///< Function values
    std::vector<double> M; ///< Second derivatives
public:
    /**
     * @brief Constructor, accepts knot vector, function values, and first derivatives at both ends
     * @param knots Knot vector
     * @param values Function values
     * @param derivative_start First derivative at the start
     * @param derivative_end First derivative at the end
     */
    CompleteCubicPPForm(const std::vector<double>& knots, const std::vector<double>& values, double derivative_start, double derivative_end)
        : Spline(knots, 3), values_(values), M(knots.size()) {
        if (knots.size() != values.size()) {
            throw std::invalid_argument("Knots and values vectors must have the same size.");
        }

        int N = knots.size();
        std::vector<double> A(N * N, 0.0);
        std::vector<double> b(N, 0.0);
        std::vector<double> h(N-1, 0.0);
        for(int i = 0; i < N - 1; ++i){
            h[i] = knots[i+1] - knots[i];
        }
        // Construct the linear system
        A[0] = 2.0;
        A[1] = 1.0;
        b[0] = 6.0 / h[0] * ((values[1] - values[0]) / h[0] - derivative_start);

        A[N * N - 2] = 1.0;
        A[N * N - 1] = 2.0;
        b[N-1] = 6.0 / h[N-2] * (derivative_end - (values[N-1] - values[N-2]) / h[N-2]);

        for (int i = 1; i < N - 1; ++i) {
            A[i * N + i - 1] = h[i-1] / (h[i-1] + h[i]);
            A[i * N + i] = 2.0;
            A[i * N + i + 1] = h[i] / (h[i-1] + h[i]);
            b[i] = 6.0 * ((values[i+1] - values[i]) / h[i] - (values[i] - values[i-1]) / h[i-1]) / (h[i-1] + h[i]);
        }

        // Solve the linear system
        M = LinearSolve(A, b);
    }

    /**
     * @brief Constructor, accepts knot vector and function
     * @param knots Knot vector
     * @param func Function
     */
    CompleteCubicPPForm(const std::vector<double>& knots, const Function& func)
        : CompleteCubicPPForm(knots, GenerateValues(knots, func), func.derivative(knots.front()), func.derivative(knots.back())) {}

    /**
     * @brief Implement the evaluate function to compute the piecewise polynomial value at x
     * @param x Input point
     * @return Piecewise polynomial value
     */
    double evaluate(double x) const override {
        
        int segment = -1;
        for (size_t i = 0; i < knots_.size() - 1; ++i) {
            if (x >= knots_[i] && x < knots_[i + 1]) {
                segment = i;
                break;
            }
        }
        if (segment == -1) {
            if (x < knots_.front() - eps || x > knots_.back() + eps) {
                throw std::out_of_range("x is out of the knots range.");
            } else if (x < knots_.front()) {
                segment = 0;
            } else {
                segment = knots_.size() - 2;
            }
        }

        double x0 = knots_[segment];
        double x1 = knots_[segment + 1];
        double y0 = values_[segment];
        double y1 = values_[segment + 1];
        double M0 = M[segment];
        double M1 = M[segment + 1];
        double h = x1 - x0;

        return M0 * (x1 - x) * (x1 - x) * (x1 - x) / (6 * h) + M1 * (x - x0) * (x - x0) * (x - x0) / (6 * h) + (y0 - M0 * h * h / 6) * (x1 - x) / h + (y1 - M1 * h * h / 6) * (x - x0) / h;
    }

private:
    /**
     * @brief Static method to generate function values
     * @param knots Knot vector
     * @param func Function
     * @return Function values
     */
    static std::vector<double> GenerateValues(const std::vector<double>& knots, const Function& func) {
        std::vector<double> values(knots.size());
        for (size_t i = 0; i < knots.size(); ++i) {
            values[i] = func(knots[i]);
        }
        return values;
    }
};

/**
 * @brief NaturalCubicPPForm class
 */
class NaturalCubicPPForm : public Spline {
protected:
    std::vector<double> values_; ///< Function values
    std::vector<double> M; ///< Second derivatives
public:
    /**
     * @brief Constructor, accepts knot vector and function values
     * @param knots Knot vector
     * @param values Function values
     */
    NaturalCubicPPForm(const std::vector<double>& knots, const std::vector<double>& values)
        : Spline(knots, 3), values_(values), M(knots.size()) {
        if (knots.size() != values.size()) {
            throw std::invalid_argument("Knots and values vectors must have the same size.");
        }
        
        int N = knots.size();
        std::vector<double> A(N * N, 0.0);
        std::vector<double> b(N, 0.0);
        std::vector<double> h(N-1, 0.0);
        for(int i = 0; i < N - 1; ++i){
            h[i] = knots[i+1] - knots[i];
        }
        // Construct the linear system
        A[0] = 1.0;
        A[(N-1) * N + (N-1)] = 1.0; 
        // b[0] and b[n-1] are the second derivatives at the endpoints, which are 0 for natural splines
        for (int i = 1; i < N - 1; ++i) {
            A[i * N + i - 1] = h[i-1] / (h[i-1] + h[i]);
            A[i * N + i] = 2.0;
            A[i * N + i + 1] = h[i] / (h[i-1] + h[i]);
            b[i] = 6.0 * ((values[i+1] - values[i]) / h[i] - (values[i] - values[i-1]) / h[i-1]) / (h[i-1] + h[i]);
        }

        // Solve the linear system
        M = LinearSolve(A, b);
    }

    /**
     * @brief Constructor, accepts knot vector and function
     * @param knots Knot vector
     * @param func Function
     */
    NaturalCubicPPForm(const std::vector<double>& knots, const Function& func)
        : NaturalCubicPPForm(knots, GenerateValues(knots, func)) {}

    /**
     * @brief Implement the evaluate function to compute the piecewise polynomial value at x
     * @param x Input point
     * @return Piecewise polynomial value
     */
    double evaluate(double x) const override {
        
        int segment = -1;
        for (size_t i = 0; i < knots_.size() - 1; ++i) {
            if (x >= knots_[i] && x < knots_[i + 1]) {
                segment = i;
                break;
            }
        }
        if (segment == -1) {
            if (x < knots_.front() - eps || x > knots_.back() + eps) {
                throw std::out_of_range("x is out of the knots range.");
            } else if (x < knots_.front()) {
                segment = 0;
            } else {
                segment = knots_.size() - 2;
            }
        }

        double x0 = knots_[segment];
        double x1 = knots_[segment + 1];
        double y0 = values_[segment];
        double y1 = values_[segment + 1];
        double M0 = M[segment];
        double M1 = M[segment + 1];
        double h = x1 - x0;

        return M0 * (x1 - x) * (x1 - x) * (x1 - x) / (6 * h) + M1 * (x - x0) * (x - x0) * (x - x0) / (6 * h) + (y0 - M0 * h * h / 6) * (x1 - x) / h + (y1 - M1 * h * h / 6) * (x - x0) / h;
    }

private:
    /**
     * @brief Static method to generate function values
     * @param knots Knot vector
     * @param func Function
     * @return Function values
     */
    static std::vector<double> GenerateValues(const std::vector<double>& knots, const Function& func) {
        std::vector<double> values(knots.size());
        for (size_t i = 0; i < knots.size(); ++i) {
            values[i] = func(knots[i]);
        }
        return values;
    }
};

/**
 * @brief PeriodicCubicPPForm class.
 */
class PeriodicCubicPPForm : public Spline {
protected:
    std::vector<double> values_; ///< Function values
    std::vector<double> M; ///< Second derivatives
public:
    /**
     * @brief Constructor, accepts knot vector and function values
     * @param knots Knot vector
     * @param values Function values
     */
    PeriodicCubicPPForm(const std::vector<double>& knots, const std::vector<double>& values)
        : Spline(knots, 3), values_(values), M(knots.size()) {
        if (knots.size() != values.size()) {
            throw std::invalid_argument("Knots and values vectors must have the same size.");
        }

        // If the values at the left and right nodes are different, prompt
        if (values.front() - values.back() > eps || values.front() - values.back() < -eps ) {
            std::cout<<"Warning: The first value is not equal to the last value. The spline may not be periodic."<<std::endl;
        }

        int N = knots.size();
        std::vector<double> A(N * N, 0.0);
        std::vector<double> b(N, 0.0);
        std::vector<double> h(N-1, 0.0);
        for(int i = 0; i < N - 1; ++i){
            h[i] = knots[i+1] - knots[i];
        }
        // Construct the linear system
        A[0] = 1.0;
        A[N-1] = -1;
        A[N * (N-1) + 1] = h[0] / (h[0] + h[N-2]);
        A[N * N - 2] = 1 - A[N * (N-1) + 1];
        A[N * N - 1] = 2.0;
        b[N - 1] = 6.0 * ((values[1] - values[0]) / h[0] - (values[N-1] - values[N-2]) / h[N-2]) / (h[0] + h[N-2]);
        for (int i = 1; i < N - 1; ++i) {
            A[i * N + i - 1] = h[i-1] / (h[i-1] + h[i]);
            A[i * N + i] = 2.0;
            A[i * N + i + 1] = h[i] / (h[i-1] + h[i]);
            b[i] = 6.0 * ((values[i+1] - values[i]) / h[i] - (values[i] - values[i-1]) / h[i-1]) / (h[i-1] + h[i]);
        }

        // Solve the linear system
        M = LinearSolve(A, b);
    }

    /**
     * @brief Constructor, accepts knot vector and function
     * @param knots Knot vector
     * @param func Function
     */
    PeriodicCubicPPForm(const std::vector<double>& knots, const Function& func)
        : PeriodicCubicPPForm(knots, GenerateValues(knots, func)) {}

    /**
     * @brief Implement the evaluate function to compute the piecewise polynomial value at x
     * @param x Input point
     * @return Piecewise polynomial value
     */
    double evaluate(double x) const override {
        
        int segment = -1;
        for (size_t i = 0; i < knots_.size() - 1; ++i) {
            if (x >= knots_[i] && x < knots_[i + 1]) {
                segment = i;
                break;
            }
        }
        if (segment == -1) {
            if (x < knots_.front() - eps || x > knots_.back() + eps) {
                throw std::out_of_range("x is out of the knots range.");
            } else if (x < knots_.front()) {
                segment = 0;
            } else {
                segment = knots_.size() - 2;
            }
        }

        double x0 = knots_[segment];
        double x1 = knots_[segment + 1];
        double y0 = values_[segment];
        double y1 = values_[segment + 1];
        double M0 = M[segment];
        double M1 = M[segment + 1];
        double h = x1 - x0;

        return M0 * (x1 - x) * (x1 - x) * (x1 - x) / (6 * h) + M1 * (x - x0) * (x - x0) * (x - x0) / (6 * h) + (y0 - M0 * h * h / 6) * (x1 - x) / h + (y1 - M1 * h * h / 6) * (x - x0) / h;
    }

private:
    /**
     * @brief Static method to generate function values
     * @param knots Knot vector
     * @param func Function
     * @return Function values
     */
    static std::vector<double> GenerateValues(const std::vector<double>& knots, const Function& func) {
        std::vector<double> values(knots.size());
        for (size_t i = 0; i < knots.size(); ++i) {
            values[i] = func(knots[i]);
        }
        return values;
    }
};

/**
 * @brief CompleteCubicBSpline class
 */
class CompleteCubicBSpline : public BSpline {
public:
    /**
     * @brief Constructor, accepts knot vector, function values, and first derivatives at both ends
     * @param knots Knot vector
     * @param values Function values
     * @param derivative_start First derivative at the start
     * @param derivative_end First derivative at the end
     */
    CompleteCubicBSpline(const std::vector<double>& knots, const std::vector<double>& values, double derivative_start, double derivative_end)
        : BSpline(ExtendKnots(knots), 3, std::vector<double>(knots.size() + 2)) {
        if (knots.size() != values.size()) {
            for (double i : knots) {
                std::cout << i << " ";
            }
            std::cout << std::endl;
            for (double i : values) {
                std::cout << i << " ";
            }
            std::cout << std::endl;
            throw std::invalid_argument("Knots and values vectors must have the same size.");
        }

        int N = knots.size();
        int NN = knots_.size();
        std::vector<double> A((N + 2) * (N + 2), 0.0);
        std::vector<double> b(N + 2, 0.0);
        std::vector<double> h(NN - 1, 0.0);
        for (int i = 0; i < NN - 1; ++i) {
            h[i] = knots_[i + 1] - knots_[i];
        }

        // Construct the linear system
        double c1 = (knots_[4] - knots_[3]) / ((knots_[4] - knots_[2]) * (knots_[4] - knots_[1]));
        double c2 = (knots_[3] - knots_[2]) / ((knots_[5] - knots_[2]) * (knots_[4] - knots_[2]));
        double c3 = (knots_[N + 3] - knots_[N + 2]) / ((knots_[N + 3] - knots_[N + 1]) * (knots_[N + 3] - knots_[N]));
        double c4 = (knots_[N + 2] - knots_[N + 1]) / ((knots_[N + 4] - knots_[N + 2]) * (knots_[N + 4] - knots_[N + 1]));
        A[0] = -3 * c1;
        A[1] = 3 * c1 - 3 * c2;
        A[2] = 3 * c2;
        b[0] = derivative_start;

        A[(N + 2) * 2 - 3] = -3 * c3;
        A[(N + 2) * 2 - 2] = 3 * c3 - 3 * c4;
        A[(N + 2) * 2 - 1] = 3 * c4;
        b[1] = derivative_end;

        for (int i = 2; i < N + 2; ++i) {
            A[i * (N + 2) + i - 2] = B(i - 1, 3, knots_[i + 1], knots_);
            A[i * (N + 2) + i - 1] = B(i, 3, knots_[i + 1], knots_);
            A[i * (N + 2) + i] = B(i + 1, 3, knots_[i + 1], knots_);
            b[i] = values[i - 2];
        }

        // Solve the linear system
        std::vector<double> coeff = LinearSolve(A, b);
        std::copy(coeff.begin(), coeff.end(), coefficients_.begin() + 1);
    }

    /**
     * @brief Constructor, accepts knot vector and function
     * @param knots Knot vector
     * @param func Function
     */
    CompleteCubicBSpline(const std::vector<double>& knots, const Function& func)
        : CompleteCubicBSpline(knots, GenerateValues(knots, func), func.derivative(knots.front()), func.derivative(knots.back())) {}

private:
    /**
     * @brief Static method to generate function values
     * @param knots Knot vector
     * @param func Function
     * @return Function values
     */
    static std::vector<double> GenerateValues(const std::vector<double>& knots, const Function& func) {
        std::vector<double> values(knots.size());
        for (size_t i = 0; i < knots.size(); ++i) {
            values[i] = func(knots[i]);
        }
        return values;
    }

    /**
     * @brief Static method to extend the knot vector
     * @param knots Knot vector
     * @return Extended knot vector
     */
    static std::vector<double> ExtendKnots(const std::vector<double>& knots) {
        std::vector<double> extendedKnots(knots.size() + 3);
        double interval = knots[1] - knots[0];
        extendedKnots[0] = knots[0] - 3 * interval;
        extendedKnots[1] = knots[0] - 2 * interval;
        extendedKnots[2] = knots[0] - interval;
        std::copy(knots.begin(), knots.end(), extendedKnots.begin() + 3);
        double Last = knots.back();
        extendedKnots.push_back(Last + interval); // Add the first new knot
        extendedKnots.push_back(Last + 2 * interval); // Add the second new knot
        extendedKnots.push_back(Last + 3 * interval); // Add the third new knot
        return extendedKnots;
    }
};

/**
 * @brief NaturalCubicBSpline class
 */
class NaturalCubicBSpline : public BSpline {
public:
    /**
     * @brief Constructor, accepts knot vector and function values
     * @param knots Knot vector
     * @param values Function values
     */
    NaturalCubicBSpline(const std::vector<double>& knots, const std::vector<double>& values)
        : BSpline(ExtendKnots(knots), 3, std::vector<double>(knots.size() + 2)) {
        if (knots.size() != values.size()) {
            for (double i : knots) {
                std::cout << i << " ";
            }
            std::cout << std::endl;
            for (double i : values) {
                std::cout << i << " ";
            }
            std::cout << std::endl;
            throw std::invalid_argument("Knots and values vectors must have the same size.");
        }

        int N = knots.size();
        int NN = knots_.size();
        std::vector<double> A((N + 2) * (N + 2), 0.0);
        std::vector<double> b(N + 2, 0.0);
        std::vector<double> h(NN - 1, 0.0);
        for (int i = 0; i < NN - 1; ++i) {
            h[i] = knots_[i + 1] - knots_[i];
        }

        // Construct the linear system
        double d1 = 1 / ((knots_[4] - knots_[2]) * (knots_[4] - knots_[1]));
        double d2 = 1 / ((knots_[5] - knots_[2]) * (knots_[4] - knots_[2]));
        double d3 = 1 / ((knots_[N + 3] - knots_[N + 1]) * (knots_[N + 3] - knots_[N]));
        double d4 = 1 / ((knots_[N + 4] - knots_[N + 2]) * (knots_[N + 4] - knots_[N + 1]));
        A[0] = 6 * d1;
        A[1] = - 6 * d1 - 6 * d2;
        A[2] = 6 * d2;
        b[0] = 0;

        A[(N + 2) * 2 - 3] = 6 * d3;
        A[(N + 2) * 2 - 2] = - 6 * d3 - 6 * d4;
        A[(N + 2) * 2 - 1] = 6 * d4;
        b[1] = 0;

        for (int i = 2; i < N + 2; ++i) {
            A[i * (N + 2) + i - 2] = B(i - 1, 3, knots_[i + 1], knots_);
            A[i * (N + 2) + i - 1] = B(i, 3, knots_[i + 1], knots_);
            A[i * (N + 2) + i] = B(i + 1, 3, knots_[i + 1], knots_);
            b[i] = values[i - 2];
        }

        // Solve the linear system
        std::vector<double> coeff = LinearSolve(A, b);
        std::copy(coeff.begin(), coeff.end(), coefficients_.begin() + 1);
    }

    /**
     * @brief Constructor, accepts knot vector and function
     * @param knots Knot vector
     * @param func Function
     */
    NaturalCubicBSpline(const std::vector<double>& knots, const Function& func)
        : NaturalCubicBSpline(knots, GenerateValues(knots, func)) {}

private:
    /**
     * @brief Static method to generate function values
     * @param knots Knot vector
     * @param func Function
     * @return Function values
     */
    static std::vector<double> GenerateValues(const std::vector<double>& knots, const Function& func) {
        std::vector<double> values(knots.size());
        for (size_t i = 0; i < knots.size(); ++i) {
            values[i] = func(knots[i]);
        }
        return values;
    }

    /**
     * @brief Static method to extend the knot vector
     * @param knots Knot vector
     * @return Extended knot vector
     */
    static std::vector<double> ExtendKnots(const std::vector<double>& knots) {
        std::vector<double> extendedKnots(knots.size() + 3);
        double interval = knots[1] - knots[0];
        extendedKnots[0] = knots[0] - 3 * interval;
        extendedKnots[1] = knots[0] - 2 * interval;
        extendedKnots[2] = knots[0] - interval;
        std::copy(knots.begin(), knots.end(), extendedKnots.begin() + 3);
        double Last = knots.back();
        extendedKnots.push_back(Last + interval); // Add the first new knot
        extendedKnots.push_back(Last + 2 * interval); // Add the second new knot
        extendedKnots.push_back(Last + 3 * interval); // Add the third new knot
        return extendedKnots;
    }
};

/**
 * @brief PeriodicCubicBSpline class
 */
class PeriodicCubicBSpline : public BSpline {
public:
    /**
     * @brief Constructor, accepts knot vector and function values
     * @param knots Knot vector
     * @param values Function values
     */
    PeriodicCubicBSpline(const std::vector<double>& knots, const std::vector<double>& values)
        : BSpline(ExtendKnots(knots), 3, std::vector<double>(knots.size() + 2)) {
        if (knots.size() != values.size()) {
            for (double i : knots) {
                std::cout << i << " ";
            }
            std::cout << std::endl;
            for (double i : values) {
                std::cout << i << " ";
            }
            std::cout << std::endl;
            throw std::invalid_argument("Knots and values vectors must have the same size.");
        }

        // If the values at the left and right nodes are different, prompt
        if (values.front() - values.back() > eps || values.front() - values.back() < -eps ) {
            std::cout<<"Warning: The first value is not equal to the last value. The spline may not be periodic."<<std::endl;
        }

        int N = knots.size();
        int NN = knots_.size();
        std::vector<double> A((N + 2) * (N + 2), 0.0);
        std::vector<double> b(N + 2, 0.0);
        std::vector<double> h(NN - 1, 0.0);
        for (int i = 0; i < NN - 1; ++i) {
            h[i] = knots_[i + 1] - knots_[i];
        }

        // Construct the linear system
        double c1 = (knots_[4] - knots_[3]) / ((knots_[4] - knots_[2]) * (knots_[4] - knots_[1]));
        double c2 = (knots_[3] - knots_[2]) / ((knots_[5] - knots_[2]) * (knots_[4] - knots_[2]));
        double c3 = (knots_[N + 3] - knots_[N + 2]) / ((knots_[N + 3] - knots_[N + 1]) * (knots_[N + 3] - knots_[N]));
        double c4 = (knots_[N + 2] - knots_[N + 1]) / ((knots_[N + 4] - knots_[N + 2]) * (knots_[N + 4] - knots_[N + 1]));
        
        double d1 = 1 / ((knots_[4] - knots_[2]) * (knots_[4] - knots_[1]));
        double d2 = 1 / ((knots_[5] - knots_[2]) * (knots_[4] - knots_[2]));
        double d3 = 1 / ((knots_[N + 3] - knots_[N + 1]) * (knots_[N + 3] - knots_[N]));
        double d4 = 1 / ((knots_[N + 4] - knots_[N + 2]) * (knots_[N + 4] - knots_[N + 1]));

        A[0] = 6 * d1;
        A[1] = - 6 * d1 - 6 * d2;
        A[2] = 6 * d2;
        A[N - 1] = - 6 * d3;
        A[N] = 6 * d3 + 6 * d4;
        A[N + 1] = - 6 * d4;
        b[0] = 0;

        A[N + 2] = -3 * c1;
        A[N + 3] = 3 * c1 - 3 * c2;
        A[N + 4] = 3 * c2;
        A[(N + 2) * 2 - 3] = 3 * c3;
        A[(N + 2) * 2 - 2] = - 3 * c3 + 3 * c4;
        A[(N + 2) * 2 - 1] = - 3 * c4;
        b[1] = 0;
        
        for (int i = 2; i < N + 2; ++i) {
            A[i * (N + 2) + i - 2] = B(i - 1, 3, knots_[i + 1], knots_);
            A[i * (N + 2) + i - 1] = B(i, 3, knots_[i + 1], knots_);
            A[i * (N + 2) + i] = B(i + 1, 3, knots_[i + 1], knots_);
            b[i] = values[i - 2];
        }
        // Solve the linear system
        std::vector<double> coeff = LinearSolve(A, b);
        std::copy(coeff.begin(), coeff.end(), coefficients_.begin() + 1);
    }

    /**
     * @brief Constructor, accepts knot vector and function
     * @param knots Knot vector
     * @param func Function
     */
    PeriodicCubicBSpline(const std::vector<double>& knots, const Function& func)
        : PeriodicCubicBSpline(knots, GenerateValues(knots, func)) {}

private:
    /**
     * @brief Static method to generate function values
     * @param knots Knot vector
     * @param func Function
     * @return Function values
     */
    static std::vector<double> GenerateValues(const std::vector<double>& knots, const Function& func) {
        std::vector<double> values(knots.size());
        for (size_t i = 0; i < knots.size(); ++i) {
            values[i] = func(knots[i]);
        }
        return values;
    }

    /**
     * @brief Static method to extend the knot vector
     * @param knots Knot vector
     * @return Extended knot vector
     */
    static std::vector<double> ExtendKnots(const std::vector<double>& knots) {
        std::vector<double> extendedKnots(knots.size() + 3);
        double interval = knots[1] - knots[0];
        extendedKnots[0] = knots[0] - 3 * interval;
        extendedKnots[1] = knots[0] - 2 * interval;
        extendedKnots[2] = knots[0] - interval;
        std::copy(knots.begin(), knots.end(), extendedKnots.begin() + 3);
        double Last = knots.back();
        extendedKnots.push_back(Last + interval); // Add the first new knot
        extendedKnots.push_back(Last + 2 * interval); // Add the second new knot
        extendedKnots.push_back(Last + 3 * interval); // Add the third new knot
        return extendedKnots;
    }
};

#endif // SPLINE_HPP